import serial
import time

class MotorController:
    def __init__(self):
        self.ser = None  # Initialize serial attribute

    def initialize_serial(self, port='COM56', baudrate=115200, timeout=1):
        """Initialize the serial connection."""
        self.ser = serial.Serial(port, baudrate, timeout=timeout)

    def send_target(self, m0_target, m1_target):
        command = f'T{m0_target} {m1_target}\n'
        self.ser.write(command.encode())

    def send_mode(self, m0_mode, m1_mode):
        command = f'M{m0_mode} {m1_mode}\n'
        self.ser.write(command.encode())

    def send_pid(self, m0_p, m0_i, m0_d, m1_p, m1_i, m1_d):
        command = f'P{m0_p} {m0_i} {m0_d} {m1_p} {m1_i} {m1_d}\n'
        self.ser.write(command.encode())

    def receive_motor_data(self):
        if self.ser.in_waiting > 0:
            line = self.ser.readline().decode('utf-8').strip()
            if line.startswith('D'):
                data = line[1:].split()
                m0_angle = float(data[0])
                m0_speed = float(data[1])
                m1_angle = float(data[2])
                m1_speed = float(data[3])
                return m0_angle, m0_speed, m1_angle, m1_speed
        return None
